/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : imu.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.


 Quaternion implementation of the 'DCM filter' [Mayhony et al].

 Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
 orientation.  See my report for an overview of the use of quaternions in this application.

 gyroscope data :   ('gx', 'gy', 'gz')  [rad/sec]
 accelerometer data ('ax', 'ay', 'az') [units irrelevant since the vector is normalized]
 magnetometer data  ('mx', 'my', 'mz') [units irrelevant since the vector is normalized]


*/

#include "imu.h"
#include "pipo.h"
#include <math.h>
#include "sensors.h"

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;	// global quaternion elements representing the estimated orientation
static float q0q0 = 1, q1q1 = 0, q2q2 = 0, q3q3 = 0; // saves multiple multiplivcation
static float exInt = 0, eyInt = 0, ezInt = 0;	// scaled integral error

//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer 
// units are irrelevant as the vector is normalised.
//
//=====================================================================================================

void IMUupdate() {
	
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;         

    float ax = sensor_data[SEN_AX];
    float ay = sensor_data[SEN_AY];
    float az = sensor_data[SEN_AZ];
    float gx = sensor_data[SEN_GX]/1000;
    float gy = sensor_data[SEN_GY]/1000;
    float gz = sensor_data[SEN_GZ]/1000;
	
	float halfT = deltaT/2;
	
	// normalise the measurements
	norm = sqrt(ax*ax + ay*ay + az*az);       
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;      
	
	// estimated direction of gravity
	vx = 2*(q1*q3 - q0*q2);
	vy = 2*(q0*q1 + q2*q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;
	
	// error is sum of cross product between reference direction of field and direction measured by sensor
	ex = (ay*vz - az*vy);
	ey = (az*vx - ax*vz);
	ez = (ax*vy - ay*vx);
	
	// integral error scaled integral gain
	exInt += ex*Ki;
	eyInt += ey*Ki;
	ezInt += ez*Ki;
	
	// adjusted gyroscope measurements
	gx = gx + Kp*ex + exInt;
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;
	
	// integrate quaternion rate and normalise
	/// ex, ey, ez have been used so can now be used as tmps to save space
	ex = ((-q1*gx - q2*gy - q3*gz)*deltaT/2);
	ey = (( q0*gx + q2*gz - q3*gy)*deltaT/2);
	ez = (( q0*gy - q1*gz + q3*gx)*deltaT/2);
	q3 +=(( q0*gz + q1*gy - q2*gx)*deltaT/2);  
	q0 += ex; q1 += ey; q2 += ez;
	
	q0q0=q0*q0;q1q1=q1*q1;q2q2=q2*q2;q3q3=q3*q3;

	// normalise quaternion
	norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;
}


/*************************************************************************
 Update IMU quaternion elements - magnetic values *are* taken into account - Mayhony's DCM
*************************************************************************/

static float q0q1 = 0, q0q2 = 0, q0q3 = 0;	
static float q1q2 = 0, q1q3 = 0, q2q3 = 0;

void AHRS()
{
	//float ax,ay,az,gx,gy,gz,mx,my,mz;

float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz;
float wx = 0, wy = 0, wz = 0;
float ex = 0, ey = 0, ez = 0;


    float ax = sensor_data[SEN_AX];
    float ay = sensor_data[SEN_AY];
    float az = sensor_data[SEN_AZ];
    float mx = sensor_data[SEN_MX];
    float my = sensor_data[SEN_MY];
    float mz = sensor_data[SEN_MZ];
    float gx = sensor_data[SEN_GX]/1000;
    float gy = sensor_data[SEN_GY]/1000;
    float gz = sensor_data[SEN_GZ]/1000;

    // normalise the measurements - normalized by measurements
//    norm = sqrt(ax*ax + ay*ay + az*az);
//    ax = ax / norm;
//    ay = ay / norm;
//    az = az / norm;
//    norm = sqrt(mx*mx + my*my + mz*mz);
//    mx = mx / norm;
//    my = my / norm;
//    mz = mz / norm;

    // compute reference direction of flux
    hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
    hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
    hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
    bx = sqrt((hx*hx) + (hy*hy));
    bz = hz;

    // estimated direction of gravity and flux (v and w)
    vx = 2*(q1q3 - q0q2);
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
    wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
    wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay*vz - az*vy) + (my*wz - mz*wy);
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

    // integral error scaled integral gain
    exInt = exInt + ex*Ki;
    eyInt = eyInt + ey*Ki;
    ezInt = ezInt + ez*Ki;

    // adjusted gyroscope measurements
    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;

    // integrate quaternion rate and normalise
    vx = (-q1*gx - q2*gy - q3*gz)*deltaT/2;
    vy = (q0*gx + q2*gz - q3*gy)*deltaT/2;
    wx = (q0*gy - q1*gz + q3*gx)*deltaT/2;
    wy = (q0*gz + q1*gy - q2*gx)*deltaT/2;

    q0 += vx;
    q1 += vy;
    q2 += wx;
    q3 += wy;

    // normalise quaternion
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;


    // auxiliary variables to reduce number of repeated operations
    q0q0 = q0*q0; q0q1 = q0*q1; q0q2 = q0*q2; q0q3 = q0*q3; q1q1 = q1*q1;
    q1q2 = q1*q2; q1q3 = q1*q3; q2q2 = q2*q2; q2q3 = q2*q3; q3q3 = q3*q3;

}


///*************************************************************************
// get roll val around Z axis - psi
//*************************************************************************/
//float getAZ()
//{
//     // psi =    atan2(2*q2*q3 - 2*q1*q4, 2*q1^2 + 2*q2^2 -1)
//    return atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 -1);
//}

///*************************************************************************
// get roll val around Y axis - theta
//*************************************************************************/
//float getAY()
//{
//    // theta =   -asin(2*q2*q4 + 2*q1*q3)
//    return -asin(2*q1*q3 + 2*q0*q2);
//}

///*************************************************************************
// get roll val around X axis - phi
//*************************************************************************/
//float getAX()
//{
//    // phi =     atan2(2*q3*q4 - 2*q1*q2, 2*q1^2 + 2*q4^2 -1)
//    return atan2(2*q2*q3 - 2*q0*q1, 2*q0*q0 + 2*q3*q3 -1);
//}


//====================================================================================================
// END OF CODE
//====================================================================================================
void getAttitude() {	
	attitude[cY] =  atan2(2*q1q2 - 2*q0q3, 2*q0q0 + 2*q1q1 -1);  // yaw = rot(Z)
	attitude[cP] = -asin(2*q1q3 + 2*q0q2);                       // pitch = rot(y)
	attitude[cR] =  atan2(2*q2q3 - 2*q0q1, 2*q0*q0 + 2*q3q3 -1); // roll = rot(x)
}